#include "JointIO.h"
#include <cassert>
#include <vector>
#include <string>
#include <iostream>


 static MapTable writeMap;
 int JointIO_getNumJoint(JointIOHandle* hjio)
 {
	return ((JointIO*)hjio)->numJoint;
 }

 void JointIO_create(JointIOHandle* hjio )
 {
	*hjio = (JointIOHandle*)new JointIO(NULL,NULL,&writeMap);
 }

 void JointIO_destroy(JointIOHandle hjio)
 {
	 delete (JointIO*)hjio;
 }

 void JointIO_readJoints(JointIOHandle hjio, double *pot, double *psA, double* psB, double* extra, const int sz)
 {
	 ((JointIO*)hjio)->readJoints(pot, psA, psB, extra, sz);
 }

 void JointIO_read(JointIOHandle hjio, double* buffer)
 {
	 ((JointIO*)hjio)->read(buffer);
 }

 void JointIO_writeJoints(JointIOHandle hjio, const double *  torqueA, const double*  torqueB, int sz)
 {
	  ((JointIO*)hjio)->writeJoints(torqueA, torqueB, sz);
 }

  void JointIO_writeValves(JointIOHandle hjio, double *buffer, int szBuffer)
 {
	  ((JointIO*)hjio)->writeValves(buffer, szBuffer);
 }

JointIO::JointIO(MapTable* _readPSMap, MapTable* _readPOTMap, MapTable* _writeMap)
{
	//setup full input-output channels
	std::vector<std::string> inChan;
	inChan.push_back("PXI1Slot2/ai0:61"); //PS01-PS62 
	inChan.push_back("PXI1Slot3/ai52:77,PXI1Slot3/ai0:37,PXI1Slot3/ai78:79"); //PS63-PS76, POT01-POT38, AI78[Force/Switch],AI79 Flow
	daqRead=new DaqGroupIO(inChan,100,-1,6);

		
	std::vector<std::string> outChan;
	outChan.push_back("PXI1Slot4/ao0:31"); //VALVE 1-32
	outChan.push_back("PXI1Slot5/ao0:31"); //VALVE 33-64
	outChan.push_back("PXI1Slot6/ao0:11,PXI1Slot6/ao12:23"); //VALVE 65-76, VALVE77-88 (hand)
	daqWrite = new DaqGroupIO(outChan,100,2,8);


	readPSMap = _readPSMap;
	readPOTMap = _readPOTMap;
	writeMap = _writeMap;

	//assert(3*daqRead->numChan == 2*daqWrite->numChan); //each joint has 2 valves and 3 sensors (2 pressure + 1 pot)
	numJoint = daqWrite->numChan/2;
	std::cerr << "JointIO(daqRead " << daqRead->numChan <<", daqWrite " << daqWrite->numChan << ") = " << numJoint <<std::endl;
}

JointIO::~JointIO()
{
	delete daqRead;
	delete daqWrite;
	std::cerr << "~JointIO()" << std::endl;
}


int JointIO::writeJoints(const double * torqueA, const double*  torqueB, int sz)
{
	assert(sz == numJoint); //all diego joints are push/pull
	double * buffer = new double[daqWrite->numChan];
	for(int i=0; i<numJoint; i++)
	{
		buffer[i*2] = torqueA[i];
		buffer[i*2+1]= torqueB[i];
	}
	int retval=writeValves(buffer, sz*2);
	delete[] buffer;
	return retval;
}

int JointIO::writeValves(double * buffer, int szBuffer)
{
	assert(szBuffer==numJoint*2);
	if(writeMap)
	{
		for(int i=0;i<(int)daqWrite->numChan;i++)
		{
			buffer[i]=writeMap->getMappedValue(i, buffer[i]);
		}
	}
	daqWrite->write(buffer);
	return 0;
}

int JointIO::read(double* buffer)
{
		return daqRead->read(buffer);
}

int JointIO::readJoints(double *pot, double *psA, double* psB, double* extra, const int sz)
{
	assert(sz==numJoint);
	double *buffer = new double[daqRead->numChan];
	read(buffer);
	for(int i=0;i<numJoint;i++)
	{
		if(psA && psB)
		{
			//read Pressure(PS) sensor 
			if(readPSMap)
			{
				psA[i] = readPSMap->getMappedValue(2*i,buffer[2*i]);
				psB[i] = readPSMap->getMappedValue(2*i+1,buffer[2*i+1]);	
			}
			else
			{
				psA[i] = buffer[2*i];
				psB[i] = buffer[2*i+1];	
			}
		}

		if(pot)
		{
			//read Potentiometer (POT) 
			if (i<38)
			{	//regular joints
				if(readPOTMap)
				{
					pot[i]=readPOTMap->getMappedValue(i, buffer[88+i]);
				}
				else
				{
					pot[i]=buffer[88+i];
				}
			}
			else
			{
				//finger joints w/o POT
				pot[i] = -1;
			}
		}
	}
	if(extra)
	{
		int nExtra = daqRead->numChan - 3*numJoint;
		fprintf(stderr,"%d\n",nExtra);
		assert(nExtra>=0);
		for(int i=0; i< nExtra;i++)
			extra[i]=buffer[3*numJoint+i];
	}

	delete[] buffer;
	return 0;
}
